﻿/*=============================================================================
* File Name : Quaternion.cpp                         Ver : 1.00  Date : 2009-3-25
*
* Description :
*
*       クォータニオン計算　ソース
*
* Author : 土田　悠貴.
*============================================================================*/
#include "Quaternion.h"
#include "Math.h"
#include "Vector3.h"
#include "Matrix3.h"
#include "Matrix4.h"
#include <cmath>

namespace tglib
{

/*===========================================================================*/
/**
 * コンストラクタ．
 */
/*===========================================================================*/
Quaternion::Quaternion() : x( 0.0f ), y( 0.0f ), z( 0.0f ), w( 0.0f )
{}

/*===========================================================================*/
/**
 * コンストラクタ．
 *
 * @param [in]  X x成分．
 * @param [in]  Y y成分．
 * @param [in]  Z z成分．
 * @param [in]  W w成分．
 */
Quaternion::Quaternion( float X, float Y, float Z, float W ) :
	x( X ), y( Y ), z( Z ), w( W )
{}

/*===========================================================================*/
/**
 * コンストラクタ．
 *
 * @param [in]  angle 角度．
 * @param [in]  axis  xyz成分．
 */
Quaternion::Quaternion( float angle, const Vector3& axis )
{
	setRotate( angle, axis.x, axis.y, axis.z );
}

/*===========================================================================*/
/**
 * コンストラクタ．
 *
 * @param [in]  m 3行3列の行列．
 */
Quaternion::Quaternion( const Matrix3& m )
{
	setMatrix( m );
}

/*===========================================================================*/
/**
 * コンストラクタ．
 *
 * @param [in]  m 4行4列の行列．
 */
Quaternion::Quaternion( const Matrix4& m )
{
	setMatrix( m );
}

/*===========================================================================*/
/**
 * コンストラクタ．
 *
 * @param [in]  axisX X軸の位置．
 * @param [in]  axisY Y軸の位置．
 * @param [in]  axisZ Z軸の位置．
 */
Quaternion::Quaternion( const Vector3& axisX, const Vector3& axisY, const Vector3& axisZ )
{
	Matrix3 M;

	M.setAxisX( axisX );
	M.setAxisY( axisY );
	M.setAxisZ( axisZ );

	*this = Quaternion( M );
}

/*===========================================================================*/
/**
 * 単位クォータニオン化する．
 *
 * @return 単位クォータニオン ．
 */
Quaternion& Quaternion::identity()
{
	x = 0.0f;
	y = 0.0f;
	z = 0.0f;
	w = 1.0f;

	return *this;
}

/*===========================================================================*/
/**
 * 内積を求める．
 *
 * @param [in]  q クォータニオン ．
 * @return 内積を返す ．
 */
float Quaternion::dot( const Quaternion& q ) const
{
	return x * q.x + y * q.y + z * q.z + w * q.w;
}

/*===========================================================================*/
/**
 * 長さを求める．
 *
 * @return 長さを返す ．
 */
float Quaternion::length() const
{
	return x * x + y * y + z * z + w * w;
}

/*===========================================================================*/
/**
 * クォータニオンを正規化する．
 *
 * @return 正規化したクォータニオン ．
 */
Quaternion& Quaternion::normalize()
{
	float	l;

	l = length();
	l = ( l > 0.0f ) ? ( 1.0f / l ) : 0.0f;

	x = x * l;
	y = y * l;
	z = z * l;
	w = w * l;

	return *this;
}

/*===========================================================================*/
/**
 * 逆クォータニオン化する．
 *
 * @return 逆クォータニオン ．
 */
Quaternion& Quaternion::inverse()
{
	float	l;

	l = length();
	l = ( l > 0.0f ) ? ( 1.0f / l ) : 0.0f;

	x = -x * l;
	y = -y * l;
	z = -z * l;
	w = -w * l;

	return *this;
}

/*===========================================================================*/
/**
 * 正規化したクォータニオンを取得する．
 *
 * @return 正規化したクォータニオン ．
 */
Quaternion Quaternion::getNormalized() const
{
	Quaternion	result( *this );

	result.normalize();

	return result;
}

/*===========================================================================*/
/**
 * 逆クォータニオンを取得．
 *
 * @return 逆クォータニオン ．
 */
Quaternion Quaternion::getInverse() const
{
	Quaternion	result( *this );

	result.inverse();

	return result;
}

/*===========================================================================*/
/**
 * 回転角度からクォータニオンを設定．
 *
 * @param [in]  angle 回転角度 ．
 * @param [in]  vx 回転軸のx成分 ．
 * @param [in]  vy 回転軸のy成分 ．
 * @param [in]  vz 回転軸のz成分 ．
 * @return クォータニオン ．
 */
Quaternion& Quaternion::setRotate( float angle, float vx, float vy, float vz )
{
	x = Math::Sin( angle / 2.0f ) * vx;
	y = Math::Sin( angle / 2.0f ) * vy;
	z = Math::Sin( angle / 2.0f ) * vz;
	w = Math::Cos( angle / 2.0f );

	return *this;
}

/*===========================================================================*/
/**
 * 回転角度からクォータニオンを設定．
 *
 * @param [in]  angle 回転角度 ．
 * @param [in]  axis  回転軸 ．
 * @return クォータニオン ．
 */
Quaternion& Quaternion::setRotate( float angle, const Vector3& axis )
{
	setRotate( angle, axis.x, axis.y, axis.z );

	return *this;
}
/*===========================================================================*/
/**
 * 回転角度からクォータニオンを設定．
 *
 * @param [in]  yaw   ヨー ．
 * @param [in]  pitch ピッチ ．
 * @param [in]  roll  ロール ．
 * @return クォータニオン ．
 */
/*===========================================================================*/
Quaternion& Quaternion::setRoteteYawPitchRoll( float yaw, float pitch, float roll )
{
	float SinYaw;
	float SinPitch;
	float SinRoll;
	float CosYaw;
	float CosPitch;
	float CosRoll;

	SinYaw		= Math::Sin( yaw	/ 2.0f );
	SinPitch	= Math::Sin( pitch	/ 2.0f );
	SinRoll		= Math::Sin( roll	/ 2.0f );
	CosYaw		= Math::Cos( yaw	/ 2.0f );
	CosPitch	= Math::Cos( pitch	/ 2.0f );
	CosRoll		= Math::Cos( roll	/ 2.0f );

	x = SinRoll * CosPitch * CosYaw - CosRoll * SinPitch * SinYaw;
	y = CosRoll * SinPitch * CosYaw + SinRoll * CosPitch * SinYaw;
	z = CosRoll * CosPitch * SinYaw - SinRoll * SinPitch * CosYaw;
	w = CosRoll * CosPitch * CosYaw + SinRoll * SinPitch * SinYaw;

	return *this;
}

/*===========================================================================*/
/**
 * 回転行列を設定．
 *
 * @param [in]  m 回転行列 ．
 * @return クォータニオン ．
 */
Quaternion& Quaternion::setMatrix( const Matrix3& m )
{
	float tr;
	float fourD;

	tr = m.m[0][0] + m.m[1][1] + m.m[2][2] + m.m[3][3];
	if( tr >= 1.0f )
	{
		fourD = 2.0f * Math::Sqrt( tr );
		w = fourD / 4.0f;
		x = ( m.m[1][2] - m.m[2][1] ) / fourD;
		y = ( m.m[2][0] - m.m[0][2] ) / fourD;
		z = ( m.m[0][1] - m.m[1][0] ) / fourD;
	}
	else 
	{
		float qa[4];
		int	  i, j, k;

		if( m.m[0][0] > m.m[1][1] )
		{
			i = 0;
		}
		else 
		{
			i = 1;
		}

		if( m.m[2][2] > m.m[i][i] )
		{
			i = 2;
		}

		j = ( i + 1 ) % 3;
		k = ( j + 1 ) % 3;

		tr = m.m[i][i] - m.m[j][j] - m.m[k][k] + 1.0f;

		fourD = 2.0f * Math::Sqrt( tr );

		qa[i] = fourD / 4.0f;
		qa[j] = ( m.m[j][i] + m.m[i][j] ) / fourD;
		qa[k] = ( m.m[k][i] + m.m[i][k] ) / fourD;
		qa[3] = ( m.m[j][k] - m.m[k][j] ) / fourD;

		x = qa[0];
		y = qa[1];
		z = qa[2];
		w = qa[3];
	}

	return *this;
}

/*===========================================================================*/
/**
 * 回転行列を設定．
 *
 * @param [in]  m 回転行列 ．
 * @return クォータニオン ．
 */
Quaternion& Quaternion::setMatrix( const Matrix4& m )
{
	float				tr;
	float				fourD;

	tr = m.m[0][0] + m.m[1][1] + m.m[2][2] + m.m[3][3];
	if ( tr >= 1.0f )
	{
		fourD = 2.0f * Math::Sqrt( tr );
		w = fourD / 4.0f;
		x = ( m.m[1][2] - m.m[2][1] ) / fourD;
		y = ( m.m[2][0] - m.m[0][2] ) / fourD;
		z = ( m.m[0][1] - m.m[1][0] ) / fourD;
	}
	else
	{
		float		qa[4];
		int			i, j, k;

		if ( m.m[0][0] > m.m[1][1] )
		{
			i = 0;
		}
		else
		{
			i = 1;
		}
		if ( m.m[2][2] > m.m[i][i] )
		{
			i = 2;
		}
		j = ( i + 1 ) % 3;
		k = ( j + 1 ) % 3;

		tr = m.m[i][i] - m.m[j][j] - m.m[k][k] + 1.0f;

		fourD = 2.0f * Math::Sqrt( tr );

		qa[i] = fourD / 4.0f;
		qa[j] = ( m.m[j][i] + m.m[i][j] ) / fourD;
		qa[k] = ( m.m[k][i] + m.m[i][k] ) / fourD;
		qa[3] = ( m.m[j][k] - m.m[k][j] ) / fourD;

		x = qa[0];
		y = qa[1];
		z = qa[2];
		w = qa[3];
	}

	return *this;
}

/*===========================================================================*/
/**
 * 軸を取得．
 *
 * @return 回転軸 ．
 */
Vector3 Quaternion::getAxis() const
{
	Vector3	v;
	float		angle;

	angle	= Math::Acos( w ) * 2.0f;
	v.x		= x / Math::Sin( angle / 2.0f );
	v.y		= y / Math::Sin( angle / 2.0f );
	v.z		= z / Math::Sin( angle / 2.0f );

	return v;
}

/*===========================================================================*/
/**
 * 角度を取得．
 *
 * @return 角度 ．
 */
float Quaternion::getAngle() const
{
	return Math::Acos( w ) * 2.0f;
}

/*===========================================================================*/
/**
 * ヨーを取得．
 *
 * @return ヨー ．
 */
float Quaternion::getYaw() const
{
	return Math::Atan( 2.0f * ( y * z + w * x ), w * w - x * x - y * y + z * z );
}

/*===========================================================================*/
/**
 * 、ピッチを取得．
 *
 * @return ピッチ ．
 */
float Quaternion::getPitch() const
{
	return Math::DegreeNormalize( Math::Asin( -2.0f * ( x * z - w * y ) ) );
}

/*===========================================================================*/
/**
 * ロールを取得．
 *
 * @return ロール ．
 */
float Quaternion::getRoll() const
{
	return Math::Atan( 2.0f * ( x * y + w * z ), w * w - x * x - y * y - z * z );
}

/*===========================================================================*/
/**
 * 球面線形補間．
 *
 * @param [in]  q1 クォータニオン ．
 * @param [in]  q2 クォータニオン ．
 * @param [in]  t 補間値 ．
 * @return 補間されたクォータニオン ．
 */
Quaternion& Quaternion::slerp( const Quaternion& q1, const Quaternion& q2, float t )
{
	Quaternion		tb;
	float			CosTheta;
	float			Beta;

	CosTheta = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;

	if( CosTheta < 0.0f )
	{
		CosTheta = -CosTheta;
		tb.x	 = -q2.x;
		tb.y	 = -q2.y;
		tb.z	 = -q2.z;
		tb.w	 = -q2.w;
	} 
	else 
	{
		tb = q2;
	}

	Beta = 1.0f - t;

	if( ( 1.0f - CosTheta ) > 0.001f )
	{
		float Theta;

		Theta = static_cast<float>( std::acos( CosTheta ) );

		Beta = static_cast<float>( std::sin( Theta * Beta ) / std::sin( Theta ) );
		t	 = static_cast<float>( std::sin( Theta * t	  ) / std::sin( Theta ) );
	}

	x = Beta * q1.x + t * tb.x;
	y = Beta * q1.y + t * tb.y;
	z = Beta * q1.z + t * tb.z;
	w = Beta * q1.w + t * tb.w;

	normalize();

	return *this;
}

/*===========================================================================*/
/**
 * 線形補間を行う．
 *
 * @param [in]  q1 クォータニオン ．
 * @param [in]  q2 クォータニオン ．
 * @param [in]  t 補間値 ．
 * @return 補間されたクォータニオン ．
 */
Quaternion& Quaternion::lerp( const Quaternion& q1, const Quaternion& q2, float t )
{
	x = Math::LERP( t, q1.x, q2.x );
	y = Math::LERP( t, q1.y, q2.y );
	z = Math::LERP( t, q1.z, q2.z );
	w = Math::LERP( t, q1.w, q2.w );

	normalize();

	return *this;
}

/*===========================================================================*/
/**
 * (+)単項演算子オーバーロード．
 *
 * @return 結果のQuaternion．
 */
Quaternion Quaternion::operator + () const
{
	return *this;
}

/*===========================================================================*/
/**
 * (-)単項演算子オーバーロード．
 *
 * @return 結果のQuaternion．
 */
Quaternion Quaternion::operator - () const
{
	return Quaternion( -x, -y, -z, -w );
}

/*===========================================================================*/
/**
 * (+=) 演算子オーバーロード．
 * 
 * @param [in]  q Quaternion．
 * @return 結果のQuaternion．
 */
Quaternion& Quaternion::operator += ( const Quaternion& q )
{
	x = x + q.x;
	y = y + q.y;
	z = z + q.z;
	w = w + q.w;

	return *this;
}

/*===========================================================================*/
/**
 * (-=) 演算子オーバーロード．
 * 
 * @param [in]  q Quaternion．
 * @return 結果のQuaternion．
 */
Quaternion& Quaternion::operator -= ( const Quaternion& q )
{
	x = x - q.x;
	y = y - q.y;
	z = z - q.z;
	w = w - q.w;

	return *this;
}

/*===========================================================================*/
/**
 * (*=) 演算子オーバーロード．
 * 
 * @param [in]  q Quaternion．
 * @return 結果のQuaternion．
 */
Quaternion& Quaternion::operator *= ( const Quaternion& q )
{
	Quaternion	tmp;

	tmp.x =  x * q.w + y * q.z - z * q.y + w * q.x;
	tmp.y = -x * q.z + y * q.w + z * q.x + w * q.y;
	tmp.z =  x * q.y - y * q.x + z * q.w + w * q.z;
	tmp.w = -x * q.x - y * q.y - z * q.z + w * q.w;

	*this = tmp;

	return *this;
}

/*===========================================================================*/
/**
 * (*=) 演算子オーバーロード．
 * 
 * @param [in]  s スカラー．
 * @return 結果のQuaternion．
 */
Quaternion& Quaternion::operator *= ( float s )
{
	x = x * s;
	y = y * s;
	z = z * s;
	w = w * s;

	return *this;
}

/*===========================================================================*/
/**
 * (/=) 演算子オーバーロード．
 * 
 * @param [in]  s スカラー．
 * @return 結果のQuaternion．
 */
Quaternion& Quaternion::operator /= ( float s )
{
	x = x / s;
	y = y / s;
	z = z / s;
	w = w / s;

	return *this;
}

/*===========================================================================*/
/**
 * (+)演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  q2 Quaternion．
 * @return 結果のQuaternion．
 */
const Quaternion operator + ( const Quaternion& q1, const Quaternion& q2 )
{
	Quaternion	result( q1 );

	result += q2;

	return result;
}

/*===========================================================================*/
/**
 * (-)演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  q2 Quaternion．
 * @return 結果のQuaternion．
 */
const Quaternion operator - ( const Quaternion& q1, const Quaternion& q2 )
{
	Quaternion	result( q1 );

	result -= q2;

	return result;
}

/*===========================================================================*/
/**
 * (*)演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  q2 Quaternion．
 * @return 結果のQuaternion．
 */
const Quaternion operator * ( const Quaternion& q1, const Quaternion& q2 )
{
	Quaternion	result( q1 );

	result	*= q2;

	return result;
}

/*===========================================================================*/
/**
 * (*)演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  s スカラー．
 * @return 結果のQuaternion．
 */
const Quaternion operator * ( const Quaternion& q, const float s )
{
	Quaternion	result( q );

	result *= s;

	return result;
}

/*===========================================================================*/
/**
 * (*)演算子オーバーロード．
 * 
 * @param [in]  s スカラー．
 * @param [in]  q1 Quaternion．
 * @return 結果のQuaternion．
 */
const Quaternion operator * ( const float s, const Quaternion& q )
{
	return q * s;
}

/*===========================================================================*/
/**
 * (/)演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  s スカラー．
 * @return 結果のQuaternion．
 */
const Quaternion operator / ( const Quaternion& q, float s )
{
	Quaternion result( q );

	result /= s;

	return result;
}

/*===========================================================================*/
/**
 * (==) 演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  q2 Quaternion．
 * @return 比較結果．
 */
bool operator == ( const Quaternion& q1, const Quaternion& q2 )
{
	return ( q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w );
}

/*===========================================================================*/
/**
 * (!=) 演算子オーバーロード．
 * 
 * @param [in]  q1 Quaternion．
 * @param [in]  q2 Quaternion．
 * @return 比較結果．
 */
bool operator != ( const Quaternion& q1, const Quaternion& q2 )
{
	return !( q1 == q2 );
}

} // namespace tglib

/*========= End of File =====================================================*/
